#include "sensors.h"
#include <util/delay.h>
#include "structures.h"
#include <avr/interrupt.h>
#include "FreeRTOS.h"
#include "task.h"
#include <util/delay.h>


#define BATT_PORT PORTD
#define BATT_DDR DDRD
/**
 * @brief Pin where the battery voltage will be readed.
 */
#define BATT_PIN PD7

//ADC value when ADC0 reaches 2,6 V (battery power = 5,2 V)
#define ADC_BATT_100 150

#if NXBOT_USE_12VBATT==1
//Value of ADC when battState should report 0%
#define ADC_BATT_0 64 
//Value of ADC when Batt. Led should be RED (No battery).
#define ADC_BATT_LIMIT 69
//Values of ADC higher than this should report 100% in battState
#define BATT_VOLT_MAX 1200
#define BATT_VOLT_MIN 1000
#else
//Value of ADC when battState should report 0%
#define ADC_BATT_0 134
//Value of ADC when Batt. Led should be RED (No battery).
#define ADC_BATT_LIMIT 142
#define BATT_VOLT_MAX 600
#define BATT_VOLT_MIN 500
#endif

#define BATT_LINEALIZE 4


//variable that reports amount of power, from 0% to 100%
static unsigned char battState;
//variable that reports amount of power, in volts. From 0 to 600 (assuming 6V battery) or 0 to 1200 (assuming 12V battery)
static unsigned int battVoltage;


void battery_init()
{
	BATT_DDR |= (1<<BATT_PIN);
}

void battery_ledEnable(unsigned char en)
{
	if(en)
		BATT_PORT |= (1<<BATT_PIN);
	else
		BATT_PORT &= ~(1<<BATT_PIN);
}

unsigned char battery_getState()
{
	return battState;
}


unsigned int battery_getVoltage()
{
	return battVoltage;
}

unsigned int battery_read(unsigned char type)
{
	if(type == BATT_READ_VOLT)
		return battVoltage;
	else
		return battState;
		
}

static void battery_convertFromADC(unsigned char adcReading)
{

	//battery voltage = ADCH * 4. BATT_LINEALIZE
	#if NXBOT_USE_12VBATT==1
	battVoltage = ((adcReading - BATT_LINEALIZE)<<3);	
	#else
	battVoltage = ((adcReading - BATT_LINEALIZE)<<2);	
	#endif		
	
	//when battery is full
	if(adcReading>ADC_BATT_100)
		battState = 100;
	//when battery is empty
	else if(adcReading<ADC_BATT_0)
		battState = 0;
	else
		#if NXBOT_USE_12VBATT==1
		battState = (char)((battVoltage>>2)-BATT_VOLT_MIN);			
		#else
		battState = (char)(battVoltage-BATT_VOLT_MIN);	
		#endif

	//inform battery state to user
	if(adcReading<=ADC_BATT_LIMIT)
		battery_ledEnable(OFF);
	else
		battery_ledEnable(ON);

}


void sensors_init(void){
	ir_init();
	ir_activateAll(ON);
	
	bumpers_init();
	
	adc_init();
	battery_init();
	sonar_init();
	
	dio_init();
}

void sensors_readAll(sensors_t *sens)
{
	static unsigned char chann = 0;
	unsigned char battADC = 0;
	
	sens->sensorsIR = sensors_getIR();
	sens->bumpers = sensors_getBumpers();
	
	
	//leer el estado de la bateria
	battADC = adc_read(0);
	//actualiza las variables utilizadas para la bateria
	battery_convertFromADC(battADC);
				
	sens->batt.battState = sensors_getBattState();
	sens->batt.battVoltage = sensors_getBattVoltage();
	
	sens->dioIn = sensors_getDIO();
	sensors_setDIO(sens->dioOut);
	
	sens->adc[chann] = adc_read(chann);
	
	if(chann<7)
		chann++;
	else
		chann = 0;
	
	//cambiar el canal ADC
	sensors_setADCChannel(chann);
}
